#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <time.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <errno.h>
#include <fcntl.h>

#include "drv_gpio.h"
#include "drv_servo.h"


long int start_position = 0;
long int start_period = 20000000;

void InitServos( void )
{	
	char buf [100];
	int fd = 0;
	int index_pwm = 0;
	for( index_pwm = 0 ; index_pwm < 4 ; index_pwm++ )
	{
		fd = open("/sys/class/pwm/pwmchip0/export", O_WRONLY);	
		if( -1 != fd)
		{
			sprintf(buf, "%d", index_pwm); 
			if( -1 != write(fd, buf, strlen(buf)))
			close(fd);
		}

		sprintf(buf, "/sys/class/pwm/pwmchip0/pwm%d/polarity", index_pwm);
		fd = open(buf, O_WRONLY);
		if( -1 != fd)
		{
			if( -1 != write(fd, "inversed", strlen("inversed")))
			close(fd);
		}

		sprintf(buf, "/sys/class/pwm/pwmchip0/pwm%d/period", index_pwm);
		fd = open(buf, O_WRONLY);
		if( -1 != fd)
		{
			sprintf(buf, "%ld", start_period);
			if( -1 != write(fd, buf, strlen(buf)))
			close(fd);
		}	

		sprintf(buf, "/sys/class/pwm/pwmchip0/pwm%d/duty_cycle", index_pwm);
		fd = open(buf, O_WRONLY);
		if( -1 != fd)
		{	
			sprintf(buf, "%ld", start_position);
			if( -1 != write(fd, buf, strlen(buf)))
			close(fd);
		}

		sprintf(buf, "/sys/class/pwm/pwmchip0/pwm%d/enable", index_pwm);
		if( -1 != fd)
		{
			fd = open(buf, O_WRONLY);
			if( -1 != write(fd, "1", 1))
			close(fd);
		}
	}
}

void DeInitServos( void )
{	
	char buf [100];
	int fd = 0;
	int index_pwm = 0;
	for( index_pwm = 0 ; index_pwm < 4 ; index_pwm++ )
	{
		fd = open("/sys/class/pwm/pwmchip0/unexport", O_WRONLY);	
		if( -1 != fd)
		{
			sprintf(buf, "%d", index_pwm); 
			if( -1 != write(fd, buf, strlen(buf)))
			close(fd);
		}
	}
}

void ServosSetPosition( int servo_id ,long int position )
{
	char buf [100];
	int fd = 0;
	sprintf(buf, "/sys/class/pwm/pwmchip0/pwm%d/duty_cycle", servo_id);
	fd = open(buf, O_WRONLY);
	if( -1 != fd)
	{
		sprintf(buf, "%ld", position);
		if( -1 != write(fd, buf, strlen(buf)))
		close(fd);
	}
}


